/*************************************************
封装机械臂转动函数部分
包含机械臂初始角度，机械臂舵机数量，
机械臂舵机转动延时参数，机械臂多个舵机同时转动函数.  
**************************************************/

#define servo_num 4
#define servo_step_delay 15
#define servo_action_delay_time 500

float servel_parts = 100.0;        //设定舵机舵机份数
int value_init_int[servo_num] = {51, 137, 159, 25};//舵机初始角度int类型
float value_init_float[servo_num] = {51, 137, 159, 25};//舵机初始角度


//机械臂初始化角度51,137,159,25
//机械臂抓取第一个盘子物体20,79,150,90
//机械臂抓取第二个盘子物体86,79,150,90
//机械臂抓取第三个盘子物体144,79,150,90

//机械臂初始化函数
void arm_init(){
  servo.init();
  servo.move(value_init_int[0], value_init_int[1], value_init_int[2], value_init_int[3]);
  delay(1000);
  //servo_action_test(); //机械臂保持初始动作
}

//多个舵机同时动作函数
void servo_move(float value0, float value1, float value2, float value3, int time_action_delay){
  float value_arguments[] = {value0, value1, value2, value3};
  float value_delta[servo_num];

  //Serial.println("---------------------------------------------------------");
  for(int i=0;i<servo_num;i++){
    value_delta[i] = (value_arguments[i] - value_init_float[i]) / servel_parts;
    //Serial.print(value_delta[i]);Serial.print(" ");
  }
  //Serial.println();

  for(int i=0;i<servel_parts;i++){
    for(int k=0;k<servo_num;k++){
      value_init_float[k] = value_delta[k] == 0 ? value_arguments[k] : value_init_float[k] + value_delta[k];
      value_init_int[k] = int(value_init_float[k]);
      //Serial.print(value_init_int[k]);Serial.print(" ");
    }
    //Serial.println();

    servo.move(value_init_int[0], value_init_int[1], value_init_int[2], value_init_int[3]);
    delay(servo_step_delay);
  }
  delay(time_action_delay);
}

//机械臂初始化动作
void arm_keep_init_state(){
  servo_move(51, 139, 159, 25, 500); //机械臂初始化动作
}

//机械臂向下转动一定的角度，使视觉模块能看到二维码
void arm_move_down_angle(){
  servo_move(51, 100, 159, 25, 1500);
}

//机械臂转动到识别物料颜色的角度
void arm_move_to_detect_angle(){
  servo_move(51, 73, 159, 25, 1500);  //机械臂识别颜色动作
}

//机械臂抓取物料
void arm_catch_material(){
  servo_move(51, 73, 156, 70, 1500);  //机械臂抓取物料
}

//舵机动作测试
void servo_action_test(){
  // servo_move(51, 139, 159, 19, 1500); //机械臂初始化动作
  servo_move(51, 73, 156, 25, 1500);  //机械臂识别颜色动作
}


//2025-2-12 机械臂抓取物料动作
void arm_catch_color_something(){
  servo_move(51, 45, 63, 25, 1500); //机械臂抓取物料动作
}